#include "Robot.h"

robot :: robot()
{
	//testE = new PIDEncoder(LFENC_A, LFENC_B, false, Encoder::k4X);
	//testE->Start();
	
	//Initialize the PIDEncoders (and PIDControllers)
	InitPIDEncoders();
	timer = new Timer();
	
	//This sets the target distance (setpoint) for all 4 encoders (arbitrary unit)
	//SetSetpoints(10.0);
	
	screen = DriverStationLCD::GetInstance();
	screen->PrintfLine(DriverStationLCD::kUser_Line1, "Test = %d", 123);
	screen->UpdateLCD();	
	
	//Initialize the Joysticks
	InitJoysticks();
	//Initialize the Motors
	InitJaguars();
	//Watchdog
	GetWatchdog().SetExpiration(0.1);
}

void robot :: InitJoysticks()
{
	stickDriveLeft = new Joystick(SDLEFT);
	stickDriveRight = new Joystick(SDRIGHT);
	stickManip1 = new Joystick(SMANIP1);
	stickManip2 = new Joystick(SMANIP2);
}

void robot :: InitJaguars()
{
	leftFrontMotor = new Jaguar(LEFTFRONT);
	leftBackMotor = new Jaguar(LEFTBACK);
	rightFrontMotor = new Jaguar(RIGHTFRONT);
	rightBackMotor = new Jaguar(RIGHTBACK);	
}

// This will be solely used for the PIDEncoders
// Setpoint, in this case, will be the distance we want the robot to travel
void robot :: SetSetpoints(double setPoint) 
{
	leftFrontPID->SetSetpoint(setPoint); //example
	leftBackPID->SetSetpoint(setPoint);
	rightFrontPID->SetSetpoint(setPoint); //example
	rightBackPID->SetSetpoint(setPoint);	
}

// The universal movement method
void robot :: SetLeftRightSideSpeeds(float speedLeft, float speedRight)
{
	leftFrontMotor->Set(speedLeft);
	leftBackMotor->Set(speedLeft);
	rightFrontMotor->Set(-speedRight);
	rightBackMotor->Set(-speedRight);
}

// Mainly for autonomous
void robot :: Drive(float speedLeft, float speedRight, float time)
{
	SetLeftRightSideSpeeds(speedLeft, speedRight);
	Wait(time);
	SetLeftRightSideSpeeds(0, 0);
}

void robot :: InitPIDEncoders() 
{
	// Setup for the left front pid encoder
	//leftFrontEncoder = new Encoder(LFENC_A, LFENC_B);
	//leftFrontEncoder->SetDistancePerPulse(1); // for testing set at 1
	//leftFrontEncoder->Start();
	//leftFrontPID = new PIDController(KP,KI,KD, leftFrontEncoder, leftFrontMotor);
	
	//Setup for left back pid encoder
	leftBackEncoder = new Encoder(LBENC_A, LBENC_B);
	leftBackEncoder->SetDistancePerPulse(DISTANCE_PER_PULSE_4X);
	leftBackEncoder->Start();
	//leftBackPID = new PIDController(KP,KI,KD, leftBackEncoder, leftBackMotor);
	
	//Setup for right front pid encoder
	rightFrontEncoder = new Encoder(RFENC_A, RFENC_B);
	rightFrontEncoder->SetDistancePerPulse(DISTANCE_PER_PULSE_4X);
	rightFrontEncoder->Start();
	//rightFrontPID = new PIDController(KP,KI,KD, rightFrontEncoder, rightFrontMotor);
	
	//Setup for right back pid encoder
	//rightBackEncoder = new Encoder(RBENC_A, RBENC_B);
	//rightBackEncoder->SetDistancePerPulse(DISTANCE_PER_PULSE_4X);
	//rightBackPID = new PIDController(Kp,Ki,Kd, rightBackEncoder, rightBackMotor);

}

void robot :: OperatorControl()
{
	float speedLeft, speedRight;
	GetWatchdog().SetEnabled(true);
	
	while(IsOperatorControl())
	{
		//speedLeft = (stickDriveLeft->GetY()>0.05||stickDriveLeft->GetY()<-0.05)*(stickDriveLeft->GetY());
		speedLeft = (stickDriveRight->GetRawAxis(2)>0.05||stickDriveRight->GetRawAxis(2)<-0.05)*(stickDriveRight->GetRawAxis(2));
		speedRight = (stickDriveRight->GetRawAxis(4)>0.05||stickDriveRight->GetRawAxis(4)<-0.05)*(stickDriveRight->GetRawAxis(4));
		SetLeftRightSideSpeeds(speedLeft, speedRight);
		GetWatchdog().Feed();
	}
}

void robot :: Autonomous()
{
	GetWatchdog().SetEnabled(false);
	
	/*
	MoveWithCalibratedSpeed(0.5, 0, KP, KI, KD);
	timer->Reset();
	timer->Start();
	while (timer->Get() < 10) {
		leftBackEncoder->Reset();
		//rightFrontEncoder->Reset();
		Wait(.150);
		double currentSpeed = leftBackEncoder->GetRaw() / .150;		
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "LRate = %f", currentSpeed);
		screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
	}
	leftBackMotor->SetSpeed(0);
	rightFrontMotor->SetSpeed(0);
	*/
	
	Move(10000, KP, KI, KD);
}

//assuming robot starts from speed = 0
void robot :: Move(double distance, double Kp, double Ki, double Kd)
{
	//double counts = distance * 4.0 / DISTANCE_PER_PULSE_4X;
	leftBackEncoder->Reset();
	//rightFrontEncoder->Reset();
	//other encoders ...
	//double speed = leftBackMotor->Get();
	double speed = 0;
	double currentDist = leftBackEncoder->GetDistance();
	double waittime = .1;
	double maxspeed = 0.8;
	double increment = 0.1;
	double accelerationDist = waittime * maxspeed*(maxspeed / increment + 1)/2.0; // starting from speed 0
	if (distance / 2.0 < accelerationDist * MAX_COUNTS_PER_SEC - COUNTER_TOLERANCE)
	{
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "small");
		screen->UpdateLCD();
		while (currentDist < distance / 2.0 || currentDist > -distance / 2.0)
		{
			speed += .1;
			if (speed > maxspeed) speed = maxspeed;
			leftBackMotor->SetSpeed(speed);
			timer->Reset();
			timer->Start();
			while (timer->Get() < 0.1) {
				if (!(currentDist < distance / 2.0 || currentDist > -distance / 2.0)) break;
				currentDist = leftBackEncoder->GetDistance();
			}
			
			screen->PrintfLine(DriverStationLCD::kUser_Line2, "distance: %f", currentDist);
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "LCount = %d", leftBackEncoder->GetRaw());
			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Target1 = %f", distance);
			screen->UpdateLCD();			
		}
		while (currentDist < distance || currentDist > -distance)
		{
			speed -= .1;
			leftBackMotor->SetSpeed(speed);
			while (timer->Get() < 0.1) {
				if (!(currentDist < distance || currentDist > -distance)) break;
				currentDist = leftBackEncoder->GetDistance();
			}
			
			screen->PrintfLine(DriverStationLCD::kUser_Line2, "distance: %f", currentDist);
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "LCount = %d", leftBackEncoder->GetRaw());
			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Target2 = %f", distance);
			screen->UpdateLCD();			
		}
	}
	else 
	{
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "large");
		screen->UpdateLCD();		
		while (currentDist < distance - (accelerationDist * MAX_COUNTS_PER_SEC - COUNTER_TOLERANCE) || currentDist > -distance + (accelerationDist * MAX_COUNTS_PER_SEC - COUNTER_TOLERANCE))
		{
			speed += .1;
			if (speed > maxspeed) speed = maxspeed;
			leftBackMotor->SetSpeed(speed);
			timer->Reset();
			timer->Start();
			Wait(0.1);
			currentDist = leftBackEncoder->GetDistance();
			
			screen->PrintfLine(DriverStationLCD::kUser_Line2, "distance: %f", currentDist);
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "LCount = %d", leftBackEncoder->GetRaw());
			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Target1 = %f", distance);
			screen->UpdateLCD();			
		}
		while (currentDist < distance || currentDist > -distance)
		{
			speed -= .1;
			leftBackMotor->SetSpeed(speed);
			while (timer->Get() < 0.1) {
				if (!(currentDist < distance || currentDist > -distance)) break;
				currentDist = leftBackEncoder->GetDistance();
			}
			//double currentSpeedRF = rightFrontEncoder->GetRaw() / .100;
			
			screen->PrintfLine(DriverStationLCD::kUser_Line2, "distance: %f", currentDist);
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "LCount = %d", leftBackEncoder->GetRaw());
			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Target2 = %f", distance);
			screen->UpdateLCD();			
		}
	}
	leftBackMotor->SetSpeed(0);
}

void robot :: MoveWithCalibratedSpeed(double percentSpeed, double prev_error, double kp, double ki, double kd)
{
	//int leftBackRawCount, rightFrontRawCount;
	
	double desiredSpeed = percentSpeed * MAX_COUNTS_PER_SEC;
	double error, p_out, i_out = 0, d_out;
	leftBackMotor->SetSpeed(percentSpeed);
	
	leftBackEncoder->Reset();
	//rightFrontEncoder->Reset();
	Wait(.100);
	double currentSpeed = leftBackEncoder->GetRaw() / .100;
	//double currentSpeed = 2000;
	bool check = ((desiredSpeed - COUNTER_TOLERANCE <= currentSpeed) && (currentSpeed <= desiredSpeed + COUNTER_TOLERANCE));
	
	
	screen->PrintfLine(DriverStationLCD::kUser_Line1, "Check = %d", check);
	screen->UpdateLCD();
	while (!((desiredSpeed - COUNTER_TOLERANCE <= currentSpeed) && (currentSpeed <= desiredSpeed + COUNTER_TOLERANCE)))
	{
		//check = ((desiredSpeed - COUNTER_TOLERANCE <= currentSpeed) && (currentSpeed <= desiredSpeed + COUNTER_TOLERANCE));
		//break;
		error = desiredSpeed - currentSpeed;
		p_out = error * kp;
	    i_out += error * ki;
	    d_out = (error - prev_error) * kd;
	    currentSpeed += p_out + i_out + d_out;
	    leftBackMotor->SetSpeed(currentSpeed / MAX_COUNTS_PER_SEC);
	    prev_error = error;					
        
		leftBackEncoder->Reset();
		//rightFrontEncoder->Reset();
		Wait(.150);
		double currentSpeed = leftBackEncoder->GetRaw() / .150;
		
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "Range: %d-%d", (int) (desiredSpeed-COUNTER_TOLERANCE), (int) (desiredSpeed+COUNTER_TOLERANCE));
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "LRate = %f", currentSpeed);
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "LCount = %d", leftBackEncoder->GetRaw());
		screen->PrintfLine(DriverStationLCD::kUser_Line4, "Speed = %f", leftBackMotor->Get());
		screen->PrintfLine(DriverStationLCD::kUser_Line5, "Check = %d", !check);
		screen->UpdateLCD();	
		
		Wait(1);
		
		//double currentSpeedRF = rightFrontEncoder->GetRaw() / .100;
	}
	screen->PrintfLine(DriverStationLCD::kUser_Line6, "DONE");
	screen->UpdateLCD();
}

START_ROBOT_CLASS(robot);
